#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     liftUpSensor,   sensorTouch)
#pragma config(Sensor, S3,     liftDownSensor, sensorTouch)
#pragma config(Sensor, S4,     HTSPB,          sensorI2CCustom9V)
#pragma config(Motor,  mtr_S1_C1_1,     linearSlides,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     grabberMount,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     frontLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     rearLeft,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     frontRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     rearRight,     tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C4_1,    wristMain,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    grabberRight,         tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    grabberLeft,          tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    irPivot,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    rampLatch,            tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//B Brick

/* Ideally this program is run from the NXT brick in the event that
the robot needs to be stored after a match for another upcoming match. */

/* Program summary:

First the linear slides are raised to their highest point so later movement of other components is
unobscured by other parts of the robot. Next, the wrist is folded in, and then the arm is folded in.
The linear slides are lowered fully, the robot beeps to indicate the ramp latch is closing, the ramp latch
closes, and the robot beeps twice more to indicate that it is fully stored. */


#include "drivers/hitechnic-superpro.h"

const int rMax = 1023;
const int rMin = 407;



const int rampLatchOpen = 0;
const int rampLatchClosed = 175;

void initializeRobot ()
{
	servo [rampLatch] = rampLatchOpen;
}

task main ()

{
	initializeRobot ();

	int inputdata;
	ubyte outputdata;
	int bit;

	// Set B0 as output
	HTSPBsetupIO(HTSPB, 0x1);

	//This command raises the linear slides so future arm and wrist movement is not obscured by the main chassis.

	while (true)

	{

		motor [linearSlides] = 100;

		if (SensorValue [liftUpSensor] == 1)

		{
			break; /* This exits the while loop so it stops the linear slides' motor when
			the sensor is depressed (the loop will not run again) */
		}
	}

	motor [linearSlides] = 0;

	//Fully retract the arm

	while (true)

	{

		// Read a 10bit wide analogue value from A0
		inputdata = HTSPBreadADC(HTSPB, 2, 10);

		float wrist = (inputdata - rMin);
		wrist /= (rMax - rMin);
		servo [wristMain] = 255 * wrist;
		// Set the output bit based on the analogue input value
		bit = (inputdata/128);
		if (bit > 5) bit = 5;
		outputdata = 1 << bit;

		HTSPBwriteIO(HTSPB, outputdata);
		//wait1Msec(50);

		motor [grabberMount] = 50;

		if (inputdata == rMin)
		{
			break;
		}
	}
	motor [grabberMount] = 0;


	// Move the linear slides down to fully store the robot.

	while (true) {

		motor [linearSlides] = -100;

		if (SensorValue [liftDownSensor] == 1)

		{
			break;
		}
	}
	motor [linearSlides] = 0;

	wait1Msec (2000); /* To ensure the ramp is ready for storage there is a wait here;
	at this point manually fold it up in the case that it is still deployed. */

	//Beep (from NXT brick) once to indicate that the ramp latch will soon close.

	PlayTone (1000, 70);

	wait1Msec (2000);

	//Close the ramp latch

	servo [rampLatch] = rampLatchClosed;

	//Wait one second

	wait1Msec (1000);

	//Beep to indicate that the robot is stored; the program should end after the beeps occur.

	PlayTone (500, 50);
	wait1Msec (500);

}
